#pragma once

#include "cartographer/rcm/imu/common.h"

namespace cartographer
{
    class ImuInitialize
    {
    public:
        ImuInitialize()
        {
        }

        ImuInitialize(size_t win_min, double duration, double motion_var);

        virtual ~ImuInitialize()
        {
        }

        void AddData(const sensor::ImuData &data);

        ImuBias imu_bias_;

        inline bool Inited()
        {
            return inited_;
        }

    private:
        void PopFront(size_t size);

        void InitializeWindow();

        void InitializeStatic();

        void CalcAverage(Eigen::Vector3d &gyro_avg, Eigen::Vector3d &accel_avg, const ImuSet &set);

        bool HasMotion(const std::vector<sensor::ImuData> &set, const Eigen::Vector3d &accel_avg);

        void CalcBias(const Eigen::Vector3d &gyro_avg, const Eigen::Vector3d &accel_avg);

        bool inited_ = false;

        int skip_times_ = 0;

        int skip_count_ = 0;

        size_t win_min_ = 5;

        double duration_ = 0.03;

        double motion_var_ = 0.03;

        std::deque<sensor::ImuData> deque_;
    };
} // namespace cartographer